/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Line handling Library (for scan processing)
*
* Basic line is presented with TLine, which represents the end and start points of line (x1,y1,x2,y2)
*  
* Author: Jari Saarinen - Automation Technology Laboratory
* Last Modified: 18.2.2004	
**/

#ifndef _LINE_LIB_H
#define _LINE_LIB_H

#include <iostream>
#include <list>
#include "error_handler.h" 
#include "split_and_merge.h"
#include "laserMath.h"
#include "tpose.h"

using namespace std;

#ifndef PI
	#define PI 3.14159265359 
#endif


/**
* Structure for presenting line in form of a*x + b*y + c = 0
*/
struct __lineParam{
	float a;
	float b;
	float c;
	int numOfPoints;    // Number of points used to construct the line(a.k.a weight)
} typedef TLineParam;



/**
* Structure to present single line with end points
*/
struct __basicLine{
	float x1,y1,x2,y2;
	TLineParam p;
	int numOfPoints; // Number of points used for this line (reliability value)
} typedef TLine;

struct __linePair{
	TLine L1;
	TLine L2;
	int weight;
	float corValue;
}typedef TLinePair;


/**
* Structure to present list of lines with start and end points
**/
struct __linevector{
	TLine *Lines;
	int NumOfLines;
	int size;
}typedef TLineVector;

struct __maplinevector{
	TLineVector map;	// Line vector for map lines
	int *timesMerged;	// How many times is i:th line merged (reliability /)
	int *lastSeen;
	int *timeIndex;
} typedef TMapLineVector;

struct __linePairVector{
	TLinePair *Pairs;
	int numOfPairs;
	int size;
} typedef TLinePairVector;

// Simple line structure
typedef struct __simpleLine{
	float x1,y1,x2,y2; // line start(x1, y1) and end(x2, y2) point
	int numOfPoints;   // Number of points used for the line
}SLine;


/**
* Function prototypes
*/
// Basic line operations
float pointDistanceFromLine(float x, float y, TLine L);
TLine setLine(float x1,float y1, float x2, float y2, int NumberOfPoints);
float lineLenght(TLine l);
float lineAngle(TLine l);
float angleDiff(TLine l1, TLine l2);
float shortestDistBetweenLineEndPoints(TLine L1,TLine L2);
TLineParam convertLine2abc(TLine L);
float calculateMirekCorrelationValue(TLine l1,TLine l2);
float calculateJariMirekCorrelationValue(TLine l1,TLine l2);
TLine rotateLine(TLine L,TPose P, float alpha);
void printLine(TLine L);
int round(float v);
void calculateAngleHistogram(TLineVector L, float hist[181]);
TLine mergeTwoLines(TLine L1, TLine L2, int& ind1, int& ind2);
int isInLine(float x0,float y0,TLine Line);
TPose shortestPointFromLine(TPose pos, TLine line);


// Line vector operations
TLineVector newLineVector(int NumOfLines);
void freeLineVector(TLineVector Lines);
void addLine2Vector(TLineVector& Lines, TLine Line,int index);
TLine getLineFromVector(TLineVector Lines, int index);

TLinePair findLinePairFromVector(TLine L, TLineVector LV, int& index);
TLinePair findLinePairFromMapVector(TLine L, TLineVector LV, int& index);

TLinePairVector findLinePairVectorFromLineVectors(TLineVector LV1, TLineVector LV2);
TLinePairVector findLinePairVectorFromLineVectorsNxN(TLineVector LV1, TLineVector LV2);
TLineVector rotateLineVector(TLineVector LV, TPose P, float alpha);
void printLineVector(TLineVector LV);

// Line pair vector operations
TLinePairVector newLinePairVector(int numOfPairs);
void freeLinePairVector(TLinePairVector LPV);
float estimateAngleBetweenLinePairs(TLinePairVector LPV);

/**
 * Uses Least Squares Method to fit line into given points. 
 *
 * @param x point x-coordinates
 * @param y point y-coordinates
 * @param size size of x and y-coordinate arrays.
 * @param line fitted line parameters.
 * @return 0 if success, else failed
 */
int lsq_estimate(float *x, float *y, int size, TLineParam *line);

/**
 * Uses Least Squares Method to fit line into given points. 
 * Steps:
 * 1. Calculate average for points in x and y-direction. 
 * 2. Calculate covariance matrix for zero averaged points.
 * 3. Find eigenvector ([a b]) for covariance matrix, it tells
 *    the direction of maximum variance.
 * 4. Calculate projection for average(calculated in 1.) in the direction
 *    of eigenvector(calculated in 3.). This is the cross-section point
 *    with y-axis (C).
 * 5. Wanted line is: b*y + a*x + c = 0
 * 6. Calculate nearest points in the line for first and last point.
 *
 * For more info about the angle calculation:
 * http://lib-www.lanl.gov/numerical/bookcpdf/c11-1.pdf
 *
 * @param x point x-coordinates
 * @param y point y-coordinates
 * @param size size of x and y-coordinate arrays.
 * @param endstart line estimated
 * @return 0 if success, else failed
 */
int lsq_estimate_points(float *x, float *y, int size, TLine *endstart);

/**
 * Convert laser scan to lines. Lines are formed with following steps.
 * 1. scan measurements are filtered to remove invalid scans
 * 2. split and merge algorithm collects the points into groups
 * 3. LSQ method fits a line to these points
 * 4. Nearest points of ending and starting point is found from the line.
 * 
 * @param scan_dists laser scan distance-array of size 361.
 * @param linev extracted lines are saved in here.
 * @param minimum_number_of_points the number of points that have
 *        to be found from line so that it is classified as line.
 * @return 0 if all ok, else -1
 */
int scans2FittedLines(float *scan_dists, TLineVector *linev, int minimum_number_of_points);
int xyScan2FittedLines(float *x,float *y, int scanN, TLineVector *linev, int minimum_number_of_points);

/**
 * Converts given distance to x-y coordinates
 *
 * @param R distance array, size is 361, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int distance2xycoordinates(float *R, float *x, float *y);

/**
 * Takes two coordinates in a global coordinate system and returns 
 * differential movement compared to first scan.
 * 
 * @param pos1global Global position of first scan, array size is 3.
 * @param pos2global Global position of second scan, array size is 3.
 * @param posdif Differential movement from pos1global reference, array size is 3.
 * @return 0 if all ok, else -1
 */
int absolut2differentialPose(float *pos1global, float *pos2global, float *posdif);

#endif


 

